#if ((FEATURE_MODE == 2) || (FEATURE_MODE == 0))
#include "sal.h"
#include "sys_tsingma_trie.h"
#include "sys_tsingma_trie_v6.h"
#include "sys_usw_ipuc.h"
#include "ctc_error.h"


#define SHL(data, shift, max)   (((shift)>=(max))?0:((data)<<(shift)))
#define SHR(data, shift, max)   (((shift)>=(max))?0:((data)>>(shift)))
#define MASK(len)  (((len)>=32 || (len)==0)?0xFFFFFFFF:((1<<(len))-1))
#define BITMASK(len)   (((len)>=32)?0xFFFFFFFF:((1<<(len))-1))
#define ABS(n) ((((int32)(n)) < 0) ? -(n) : (n))


#define NALPM_BITS2WORDS(x)        (((x) + 31) >>5)
#define BITS2WORDS(x)        NALPM_BITS2WORDS(x)
#define _MAX_KEY_WORDS_     (4)
#define KEY_BIT2IDX(x) ((128 - (x))>>5)
#define BITS2SKIPOFF(x) (((x) + 30) / 31)
#define _BITOP(_a, _b, _op)   ((_a) _op (1U << ((_b) &0x1F)))
#define _BITGET(_a, _b) _BITOP(_a, _b, &)
#define _BITSET(_a, _b) _BITOP(_a, _b, |=)
#define _BITCLR(_a, _b) _BITOP(_a, _b, &= ~)


#define BITGETLSBSET(data, msb, lsb)    \
{                                    \
    lsb = 0;                         \
    while ((lsb)<=(msb)) { \
        if ((data)&(1<<(lsb)))     { \
            break;                   \
        } else { (lsb)++;}           \
    }                                \
    lsb = ((lsb)>(msb))?-1:(lsb);    \
}


/* loop up key size */
#define TAPS_IPV4_MAX_VRF_SIZE (16)
#define TAPS_IPV4_PFX_SIZE (32)
#define TAPS_IPV4_KEY_SIZE (TAPS_IPV4_MAX_VRF_SIZE + TAPS_IPV4_PFX_SIZE)
#define TAPS_IPV4_KEY_SIZE_WORDS (((TAPS_IPV4_KEY_SIZE)+31)>>5)
#define TAPS_IPV6_MAX_VRF_SIZE (16)
#define TAPS_IPV6_PFX_SIZE (128)
#define TAPS_IPV6_KEY_SIZE (128)
#define TAPS_IPV6_KEY_SIZE_WORDS (((TAPS_IPV6_KEY_SIZE)+31)>>5)
#define TAPS_MAX_KEY_SIZE       (TAPS_IPV6_KEY_SIZE)
#define TAPS_MAX_KEY_SIZE_WORDS (((TAPS_MAX_KEY_SIZE)+31)>>5)
#define TP_MASK(len)  (((len)>=32)?0xffffFFFF:((1U<<(len))-1))
#define TP_SHL(data, shift)   (((shift)>=32)?0:((data)<<(shift)))
#define TP_SHR(data, shift)  (((shift)>=32)?0:((data)>>(shift)))


#define TP_BITS2IDX(bit_pos, max_len) ((NALPM_BITS2WORDS(max_len) - 1) - ((bit_pos)>>5))
#define _TAPS_GET_KEY_BIT(key, bit_pos, max_len) (((key)[TP_BITS2IDX(bit_pos, max_len)] & (1<<((bit_pos)&0x1F))) >> ((bit_pos)&0x1F))
#define _TAPS_SET_KEY_BIT(key, bit_pos, max_len) ((key)[TP_BITS2IDX(bit_pos, max_len)] |= (1<<((bit_pos)&0x1F)))

#define _TAPS_GET_KEY_BITS(key, lsb, len, max_len)  \
  ((TP_SHR((key)[TP_BITS2IDX(lsb, max_len)], ((lsb)&0x1F)) |\
    ((TP_BITS2IDX(lsb, max_len)<1)?0:(TP_SHL((key)[TP_BITS2IDX(lsb, max_len)-1], 32-((lsb)&0x1F))))) & \
   (TP_MASK((len))))

STATIC int32
sys_nalpm_taps_key_shift (uint32 max_key_size, uint32 * key, uint32 length, int32 shift)
{
    int32 word_idx, lsb;

    if (shift > 0)
    {
        /* right shift */
        for (lsb = shift, word_idx = NALPM_BITS2WORDS (max_key_size) - 1;  word_idx >= 0; lsb += 32, word_idx--)
        {
            if (lsb < length)
            {
                key[word_idx] =  _TAPS_GET_KEY_BITS (key, lsb,  ((length - lsb) >= 32) ? 32 : (length - lsb), max_key_size);
            }
            else
            {
                key[word_idx] = 0;
            }
        }
    }
    else if (shift < 0)
    {
        /* left shift */
        shift = 0 - shift;

        /* whole words shifting first */
        for (word_idx = 0; ((shift >>5) != 0) && (word_idx < NALPM_BITS2WORDS (max_key_size));  word_idx++)
        {
            if ((word_idx + (shift >>5)) >= NALPM_BITS2WORDS (max_key_size))
            {
                key[word_idx] = 0;
            }
            else
            {
                key[word_idx] = key[word_idx + (shift >>5)];
            }
        }

        /* shifting remaining bits */
        for (word_idx = 0; ((shift &0x1F) != 0) && (word_idx < NALPM_BITS2WORDS (max_key_size));  word_idx++)
        {
            if (word_idx == TP_BITS2IDX (0, max_key_size))
            {
                /* at bit 0 word, next word doesn't exist */
                key[word_idx] = TP_SHL (key[word_idx], (shift &0x1F));
            }
            else
            {
                key[word_idx] = TP_SHL (key[word_idx], (shift &0x1F)) | TP_SHR (key[word_idx + 1], 32 - (shift &0x1F));
            }
        }

    }

    return CTC_E_NONE;
}



STATIC uint32
_key_get_bits_2 (uint32 *key,
               uint32 pos /* 1based, msb bit position */ ,
               uint32 len, uint32 skip_len_check)
{

    return _TAPS_GET_KEY_BITS (key, pos - len, len, 128);
}

int32
_key_append_v6 (uint32 *key,
             uint32 *length,
             uint32 skip_addr, uint32 skip_len)
{
    sys_nalpm_taps_key_shift (128, key, *length, 0 - (int32) skip_len);
    key[KEY_BIT2IDX (1)] |= skip_addr;
    *length += skip_len;

    return 0;
}

uint32 STATIC
_lcplen_v6 (uint32 *key, uint32 len1,
        uint32 skip_addr, uint32 len2)
{
    uint32 diff;
    uint32 lcp = len1 < len2 ? len1 : len2;

    if ((len1 == 0) || (len2 == 0))
    {
        return 0;
    }

    diff = _key_get_bits_2 (key, len1, lcp, TRUE);
    diff ^= (SHR (skip_addr, len2 - lcp, 31) & MASK (lcp));

    while (diff)
    {
        diff >>= 1;
        --lcp;
    }

    return lcp;
}

int32
sys_trie_v6_find_lpm (trie_node_t *parent,
                   trie_node_t * trie,
                   uint32 *key,
                   uint32 length,
                   trie_node_t ** payload,
                   trie_callback_f cb, void *user_data, uint32 last_total_skip_len)
{
    uint32 lcp = 0;
    int32 bit = 0, rv = CTC_E_NONE;

    if (!trie)
        return CTC_E_INVALID_PARAM;

    if (parent == trie)
    {
        /* root node */
       trie->total_skip_len = trie->skip_len;
    }
    else
    {
        /* not root node need add its parent's total skip length + slash + itself's skip length*/
        trie->total_skip_len = (last_total_skip_len + trie->skip_len + 1);
    }

    lcp = _lcplen_v6 (key, length, trie->skip_addr, trie->skip_len);

    if ((length > trie->skip_len) && (lcp == trie->skip_len))
    {
        if (trie->type == PAYLOAD)
        {
            /* lpm cases */
            if (payload != NULL && 1 != trie->dirty_flag)
            {
                /* update lpm result */
                *payload = trie;
                parent = trie;
            }
            else if(payload != NULL && 1 == trie->dirty_flag)
            {
                *payload = parent;
            }

            if (cb != NULL)
            {
                /* callback with any nodes which is shorter and matches the prefix */
                rv = cb (trie, user_data);
                if (NALPM_E_FAILURE (rv))
                {
                    /* early bailout if there is error in callback handling */
                    return rv;
                }
            }
        }

        bit = (key[KEY_BIT2IDX (length - lcp)] & SHL (1, (length - lcp - 1) &0x1F, 32)) ? 1 : 0;

        /* based on next bit branch left or right */
        if (trie->child[bit].child_node)
        {
            return sys_trie_v6_find_lpm (parent, trie->child[bit].child_node, key,
                                      length - lcp - 1, payload, cb, user_data, trie->total_skip_len);
        }
    }
    else if ((length == trie->skip_len) && (lcp == length))
    {
        if (trie->type == PAYLOAD)
        {
            /* exact match case */
            if (payload != NULL && 1 != trie->dirty_flag)
            {
                /* lpm is exact match */
                *payload = (1 == trie->dirty_flag)?parent:trie;
            }

            if (cb != NULL)
            {
                /* callback with the exact match node */
                rv = cb (trie, user_data);
                if (NALPM_E_FAILURE (rv))
                {
                    /* early bailout if there is error in callback handling */
                    return rv;
                }
            }
        }
    }
    return rv;
}

int32
sys_trie_v6_skip_node_alloc (trie_node_t ** node, uint32 *key,
                          uint32 *bpm, uint32 msb,
                          uint32 skip_len, trie_node_t * payload, uint32 count)
{
    int32 lsb = 0, msbpos = 0, lsbpos = 0, bit = 0, index;
    trie_node_t *child = NULL, *skip_node = NULL;

    lsb = ((msb) ? msb + 1 - skip_len : msb);

    if (msb)
    {
        for (index = BITS2SKIPOFF (lsb), lsbpos = lsb - 1; index <= BITS2SKIPOFF (msb); index++)
        {
             if (lsbpos == lsb - 1)
            {
                skip_node = payload;
            }
            else
            {
                skip_node = mem_malloc2(MEM_IPUC_MODULE, sizeof (trie_node_t), trie_node_t);
                if (NULL == skip_node)
                {
                    return CTC_E_NO_MEMORY;
                }
            }

            sal_memset (skip_node, 0, sizeof (trie_node_t));

            msbpos = index * 31 - 1;

            if (msbpos > msb - 1)
            {
                msbpos = msb - 1;
            }

            /* calculate the skip_len of the created node */
            if (msbpos - lsbpos < 31)
            {
                skip_node->skip_len = msbpos - lsbpos + 1;
            }
            else
            {
                skip_node->skip_len = 31;
            }

            if (NALPM_BITS2WORDS (msbpos + 1) != NALPM_BITS2WORDS (lsbpos + 1))
            {
                skip_node->skip_addr =  key[KEY_BIT2IDX (msbpos + 1)] & MASK ((msbpos + 1) &0x1F);
                skip_node->skip_addr =  SHL (skip_node->skip_addr, skip_node->skip_len - ((msbpos + 1) &0x1F), 32);
                skip_node->skip_addr |=  SHR (key[KEY_BIT2IDX (lsbpos + 1)], (lsbpos &0x1F), 32);
            }
            else
            {
                skip_node->skip_addr =  SHR (key[KEY_BIT2IDX (msbpos + 1)], (lsbpos &0x1F), 32);
            }

            if (child)
            {
                skip_node->child[bit].child_node = child;
            }

            bit = (skip_node->skip_addr & SHL (1, skip_node->skip_len - 1, 31)) ? 1 : 0;

            if (lsbpos == lsb - 1)
            {
                skip_node->type = PAYLOAD;
            }
            else
            {
                skip_node->type = INTERNAL;
            }

            skip_node->count = count;
            lsbpos += skip_node->skip_len;

            if (msbpos != msb - 1)
            {
                skip_node->skip_len--;
            }


            skip_node->skip_addr &= MASK (skip_node->skip_len);
            child = skip_node;
        }
    }
    else
    {
        skip_node = payload;
        sal_memset (skip_node, 0, sizeof (trie_node_t));
        skip_node->type = PAYLOAD;
        skip_node->count = count;

    }

    *node = skip_node;
    return CTC_E_NONE;
}

int32
sys_trie_v6_insert (trie_node_t * trie, uint32 *key,
                 uint32 *bpm, uint32 length, trie_node_t * payload, 	/* payload node */
                 trie_node_t **
                 child /* child pointer if the child is modified */ )
{
    uint32 lcp;
    int32 rv = CTC_E_NONE, bit = 0;
    trie_node_t *node = NULL;

    if (!trie || !payload || !child || (length > 128))
        return CTC_E_INVALID_PARAM;

    *child = NULL;


    lcp = _lcplen_v6 (key, length, trie->skip_addr, trie->skip_len);

    /* if the new key qualifies as new root do the inserts here */
    if (lcp == length)
    {
        /* guaranteed: length < 31 */
        if (trie->skip_len == lcp)
        {
            if (trie->type != INTERNAL)
            {
                /* duplicate */
                return CTC_E_EXIST;
            }
            else
            {
                /* change the internal node to payload node */
                _CLONE_TRIE_NODE_ (payload, trie);
                mem_free (trie);
                payload->type = PAYLOAD;
                payload->count++;
                *child = payload;

                return CTC_E_NONE;
            }
        }
        else
        {
            /* skip length can never be less than lcp implcitly here */
            /* this node is new parent for the old trie node */
            /* lcp is the new skip length */
            _CLONE_TRIE_NODE_ (payload, trie);
            *child = payload;

            bit = (trie->skip_addr & SHL (1, trie->skip_len - length - 1, 31)) ? 1 : 0;
            trie->skip_addr &= MASK (trie->skip_len - length - 1);
            trie->skip_len -= (length + 1);
            /* clone from trie node but dirty_flag should initialize as 0 */
            payload->dirty_flag = 0;

            payload->skip_addr = (length > 0) ? key[KEY_BIT2IDX (length)] : 0;
            payload->skip_addr &= MASK (length);
            payload->skip_len = length;
            payload->child[bit].child_node = trie;
            payload->child[!bit].child_node = NULL;
            payload->type = PAYLOAD;
            payload->count++;

        }
    }
    else if (lcp == trie->skip_len)
    {
        /* key length is implictly greater than lcp here */
        /* decide based on key's next applicable bit */
        bit = (key[KEY_BIT2IDX (length - lcp)] & SHL (1, (length - lcp - 1) &0x1F, 32)) ? 1 : 0;

        if (!trie->child[bit].child_node)
        {
            /* the key is going to be one of the child of existing node */
            /* should be the child */
            rv = sys_trie_v6_skip_node_alloc (&node, key, bpm, length - lcp - 1, length - lcp - 1, payload, 1);
            if (NALPM_E_SUCCESS (rv))
            {
                trie->child[bit].child_node = node;
                trie->count++;
            }

        }
        else
        {
            rv = sys_trie_v6_insert (trie->child[bit].child_node,  key, bpm, length - lcp - 1, payload, child);
            if (NALPM_E_SUCCESS (rv))
            {
                trie->count++;
                if (*child != NULL)
                {
                    /* chande the old child pointer to new child */
                    trie->child[bit].child_node = *child;
                    *child = NULL;
                }
            }
        }
    }
    else
    {
        trie_node_t *newchild = NULL;

        node = mem_malloc2(MEM_IPUC_MODULE, sizeof (trie_node_t), trie_node_t);
        if(NULL == node)
        {
            return CTC_E_NO_MEMORY;
        }
        _CLONE_TRIE_NODE_ (node, trie);
        node->dirty_flag = 0;

        rv = sys_trie_v6_skip_node_alloc (&newchild, key, bpm,
                                       ((lcp) ? length - lcp - 1 : length - 1),
                                       length - lcp - 1, payload, 1);
        if (NALPM_E_SUCCESS (rv))
        {
            bit = (key[KEY_BIT2IDX (length - lcp)] & SHL (1, (length - lcp - 1) &0x1F,  32)) ? 1 : 0;
            node->child[!bit].child_node = trie;
            node->child[bit].child_node = newchild;
            node->type = INTERNAL;
            node->skip_addr =
            SHR (trie->skip_addr, trie->skip_len - lcp, 31);
            node->skip_len = lcp;
            node->count++;
            *child = node;
            trie->skip_addr &= MASK (trie->skip_len - lcp - 1);
            trie->skip_len -= (lcp + 1);
        }
        else
        {
            mem_free (node);
        }
    }

    return rv;
}

int32
sys_trie_v6_delete (trie_node_t * trie,
                 uint32 *key,
                 uint32 length,
                 trie_node_t ** payload, trie_node_t ** child)
{
    uint32 lcp;
    int32 rv = CTC_E_NONE, bit = 0;
    trie_node_t *node = NULL;

    *child = NULL;

    lcp = _lcplen_v6 (key, length, trie->skip_addr, trie->skip_len);

    if (length > trie->skip_len)
    {
        if (lcp == trie->skip_len)
        {
            bit = (key[KEY_BIT2IDX (length - lcp)] & SHL (1, (length - lcp - 1) &0x1F, 32)) ? 1 : 0;

            if (trie->child[bit].child_node)
            {
                rv = sys_trie_v6_delete (trie->child[bit].child_node, key, length - lcp - 1, payload, child);

                if (rv == CTC_E_IN_USE)
                {
                    trie->child[bit].child_node = NULL;
                    rv = CTC_E_NONE;
                    trie->count--;

                    if (trie->type == INTERNAL)
                    {
                        bit = (bit == 0) ? 1 : 0;

                        if (trie->child[bit].child_node == NULL)
                        {
                            mem_free (trie);
                            rv = CTC_E_IN_USE;
                        }
                        else
                        {
                            if (trie->skip_len + trie->child[bit].child_node->skip_len + 1 <= 31)
                            {
                                *child = trie->child[bit].child_node;
                                rv = sys_nalpm_trie_fuse_child (trie, bit);
                                if (rv != CTC_E_NONE)
                                {
                                    *child = NULL;
                                }
                            }
                        }
                    }
                }
                else if (NALPM_E_SUCCESS (rv))
                {
                    trie->count--;

                    if (*child != NULL)
                    {
                        trie->child[bit].child_node = *child;
                        *child = NULL;
                    }
                }
            }
            else
            {
                rv = CTC_E_NOT_EXIST;
            }

        }
        else
        {
            rv = CTC_E_NOT_EXIST;
        }
    }
    else if (length == trie->skip_len)
    {
        if (!((lcp == length) && (trie->type == PAYLOAD)))
        {
            rv = CTC_E_NOT_EXIST;
        }
        else
        {
            rv = CTC_E_NONE;

            if (trie->child[0].child_node && trie->child[1].child_node)
            {
                node = mem_malloc2(MEM_IPUC_MODULE, sizeof (trie_node_t), trie_node_t);
                if (NULL == node)
                {
                    return CTC_E_NO_MEMORY;
                }
                _CLONE_TRIE_NODE_ (node, trie);
                node->type = INTERNAL;
                node->count--;
                *child = node;
            }
            else if (trie->child[0].child_node || trie->child[1].child_node)
            {
                bit = (trie->child[0].child_node) ? 0 : 1;
                trie->count--;
                if (trie->skip_len + trie->child[bit].child_node->skip_len +  1 <= 31)
                {
                    *child = trie->child[bit].child_node;
                    rv = sys_nalpm_trie_fuse_child (trie, bit);
                    if (rv != CTC_E_NONE)
                    {
                        *child = NULL;
                    }
                }
                else
                {
                    node = mem_malloc2(MEM_IPUC_MODULE, sizeof (trie_node_t), trie_node_t);
                    if(NULL == node)
                    {
                        return CTC_E_NO_MEMORY;
                    }

                    _CLONE_TRIE_NODE_ (node, trie);
                    node->type = INTERNAL;
                    *child = node;
                }
            }
            else
            {
                rv = CTC_E_IN_USE;
            }

            *payload = trie;
        }
    }
    else
    {
        rv = CTC_E_NOT_EXIST;
    }

    return rv;
}

int32
sys_trie_v6_split (trie_node_t * trie,
                uint32 *pivot,
                uint32 *length,
                uint32 *split_count,
                trie_node_t ** split_node,
                trie_node_t ** child,
                const uint32 max_count,
                const uint32 max_split_len,
                const int32 split_to_pair,
                uint32 *bpm, trie_split_states_e_t * state, const int32 max_split_count)
{
    int32 bit = 0, rv = CTC_E_NONE;

    if (trie->child[0].child_node && trie->child[1].child_node)
    {
        bit = (trie->child[0].child_node->count > trie->child[1].child_node->count) ? 0 : 1;
    }
    else
    {
        bit = (trie->child[0].child_node) ? 0 : 1;
    }

   _key_append_v6 (pivot, length, trie->skip_addr, trie->skip_len);

    if (trie->count <= max_split_count && (((*length == max_split_len) && (trie->count != max_count)) || (!trie->child[bit].child_node) ||
        (ABS (trie->child[bit].child_node->count * 2 - max_count * 1) >
    ABS (trie->count * 2 - max_count * 1))))
    {
        *split_node = trie;
        *split_count = trie->count;

        *state = TRIE_SPLIT_STATE_PRUNE_NODES;
        return rv;
    }
    else
    {
       _key_append_v6 (pivot, length, bit, 1);
        rv = sys_trie_v6_split (trie->child[bit].child_node,
                             pivot, length,
                             split_count, split_node,
                             child, max_count, max_split_len,
                             split_to_pair, bpm, state, max_split_count);
    }


    /* free up internal nodes if applicable */
    switch (*state)
    {
        case TRIE_SPLIT_STATE_PRUNE_NODES:
            if (trie->count == *split_count)
            {
                mem_free(trie);
            }
            else
            {
                trie->child[bit].child_node = NULL;
                bit = (bit == 0) ? 1 : 0;
                trie->count -= *split_count;

                if ((trie->type == INTERNAL) && (trie->skip_len + trie->child[bit].child_node->skip_len + 1 <= 31))
                {
                    *child = trie->child[bit].child_node;
                    rv = sys_nalpm_trie_fuse_child (trie, bit);
                    if (rv != CTC_E_NONE)
                    {
                        *child = NULL;
                    }
                }
                *state = TRIE_SPLIT_STATE_DONE;
            }
            break;

        case TRIE_SPLIT_STATE_DONE:

            /* update the child pointer if child was pruned */
            if (*child != NULL)
            {
                trie->child[bit].child_node = *child;
                *child = NULL;
            }
            trie->count -= *split_count;
            break;

        default:
            break;
    }

    return rv;
}


int32 _trie_v6_skip_node_free(trie_node_t *trie,
                            uint32 *key,
                            uint32 length)
{
    uint32 lcp = 0;
    int32 bit = 0, rv = CTC_E_NONE;

    lcp = _lcplen_v6(key, length, trie->skip_addr, trie->skip_len);

    if (length > trie->skip_len)
    {
        if (lcp == trie->skip_len)
        {
            bit = (key[KEY_BIT2IDX(length - lcp)] & (1 << ((length - lcp - 1) &0x1F))) ? 1:0;

            /* There should be only one branch on the chain until the end node */
            if (!trie->child[0].child_node == !trie->child[1].child_node)
            {
                return CTC_E_INVALID_PARAM;
            }

            /* based on next bit branch left or right */
            if (trie->child[bit].child_node)
            {
                rv = _trie_v6_skip_node_free(trie->child[bit].child_node, key, length - lcp - 1);
                if (NALPM_E_SUCCESS(rv))
                {
                    mem_free(trie);
                }
                return rv;
            }
        }
    }
    else if (length == trie->skip_len)
    {
        if (lcp == length)
        {
            return CTC_E_NONE;
        }
    }

    return CTC_E_NOT_EXIST; /* not found */

}
#if 0
int32
_print_v6_trie_node (trie_node_t * trie, void *datum)
{
    if (trie != NULL)
    {
        sal_printf("trie: %p, type %s, skip_addr 0x%x skip_len %d count:%d bpm:0x%x Child[0]:%p Child[1]:%p\n",
             trie, (trie->type == PAYLOAD) ? "P" : "I", trie->skip_addr,
             trie->skip_len, trie->count, trie->bpm, trie->child[0].child_node,
             trie->child[1].child_node);
    }

    return CTC_E_NONE;
}
#endif
int32 _trie_v6_search(trie_node_t *trie,
                    uint32 *key,
                    uint32 length,
                    trie_node_t **payload,
                    uint32 *result_key,
                    uint32 *result_len,
                    uint32 dump,
                    uint32 find_pivot)
{
    uint32 lcp = 0;
    int32 bit = 0;

    lcp = _lcplen_v6(key, length, trie->skip_addr, trie->skip_len);

    if (length > trie->skip_len)
    {
        if (lcp == trie->skip_len)
        {
            bit = (key[KEY_BIT2IDX(length - lcp)] & (1 << ((length - lcp - 1) &0x1F))) ? 1:0;

            if (result_key)
            {
                _key_append_v6(result_key, result_len, trie->skip_addr, trie->skip_len);
            }

            /* based on next bit branch left or right */
            if (trie->child[bit].child_node)
            {
                if (result_key)
                {
                    _key_append_v6(result_key, result_len, bit, 1);

                }

                return _trie_v6_search(trie->child[bit].child_node, key,
                                       length - lcp - 1, payload,
                                       result_key, result_len, dump, find_pivot);
            }
        }
    }
    else if (length == trie->skip_len)
    {
        if (lcp == length)
        {
            *payload = trie;
            if (trie->type != PAYLOAD && !find_pivot)
            {
                return CTC_E_NOT_EXIST;
            }
            if (result_key)
            {
                _key_append_v6(result_key, result_len, trie->skip_addr, trie->skip_len);

            }
            return CTC_E_NONE;
        }

    }
    else
    {
        if (lcp == length && find_pivot)
        {
            *payload = trie;
            if (result_key)
            {
                _key_append_v6(result_key, result_len, trie->skip_addr, trie->skip_len);

            }
         }
    }

    return CTC_E_NOT_EXIST; /* not found */

}


int32 _trie_v6_insert(trie_node_t *trie,
                uint32 *key,
                /* bpm bit map if bpm management is required, passing null skips bpm management */
                uint32 *bpm,
                uint32 length,
                trie_node_t *payload, /* payload node */
                trie_node_t **child, /* child pointer if the child is modified */
                int32 child_count)
{
    uint32 lcp;
    int32 rv = CTC_E_NONE, bit = 0;
    trie_node_t *node = NULL;


    *child = NULL;

    lcp = _lcplen_v6(key, length, trie->skip_addr, trie->skip_len);

    if (lcp == length)
    {
        if (trie->skip_len == lcp)
        {
            if (trie->type != INTERNAL)
            {
                return CTC_E_EXIST;
            }
            else
            {
                /* change the internal node to payload node */
                _CLONE_TRIE_NODE_(payload, trie);
                mem_free(trie);
                payload->type = PAYLOAD;
                payload->count += child_count;
                *child = payload;


                return CTC_E_NONE;
            }
        }
        else
        {
            _CLONE_TRIE_NODE_(payload, trie);
            *child = payload;

            bit = (trie->skip_addr & SHL(1, trie->skip_len - length - 1, 31)) ? 1 : 0;
            trie->skip_addr &= MASK(trie->skip_len - length - 1);
            trie->skip_len  -= (length + 1);

            payload->skip_addr = (length > 0) ? key[KEY_BIT2IDX(length)] : 0;
            payload->skip_addr &= MASK(length);
            payload->skip_len  = length;
            payload->child[bit].child_node = trie;
            payload->child[!bit].child_node = NULL;
            payload->type = PAYLOAD;
            payload->count += child_count;
        }
    }
    else if (lcp == trie->skip_len)
    {
        bit = (key[KEY_BIT2IDX(length - lcp)] & (1 << ((length - lcp - 1) &0x1F))) ? 1:0;

        if (!trie->child[bit].child_node)
        {
            rv = sys_trie_v6_skip_node_alloc(&node, key, bpm,
                                             length - lcp - 1, /* 0 based msbit position */
                                             length - lcp - 1,
                                             payload, child_count);
            if (NALPM_E_SUCCESS(rv))
            {
                trie->child[bit].child_node = node;
                trie->count += child_count;
            }

        }
        else
        {
            rv = _trie_v6_insert(trie->child[bit].child_node, key, bpm, length - lcp - 1,  payload, child, child_count);
            if (NALPM_E_SUCCESS(rv))
            {
                trie->count += child_count;

                if (*child != NULL)
                {
                    trie->child[bit].child_node = *child;
                    *child = NULL;
                }
            }
        }
    }
    else
    {
        trie_node_t *newchild = NULL;

        /* need to introduce internal nodes */
        node = mem_malloc2(MEM_IPUC_MODULE, sizeof(trie_node_t), trie_node_t);
        if (NULL == node)
        {
            return CTC_E_NO_MEMORY;
        }
        _CLONE_TRIE_NODE_(node, trie);

        rv = sys_trie_v6_skip_node_alloc(&newchild, key, bpm,
                                         ((lcp)?length - lcp - 1:length - 1),
                                         length - lcp - 1,
                                         payload, child_count);
        if (NALPM_E_SUCCESS(rv))
        {
            bit = (key[KEY_BIT2IDX(length - lcp)] & (1 << ((length - lcp - 1) &0x1F))) ? 1: 0;

            node->child[!bit].child_node = trie;
            node->child[bit].child_node = newchild;
            node->type = INTERNAL;
            node->skip_addr = SHR(trie->skip_addr, trie->skip_len - lcp, 31);
            node->skip_len = lcp;
            node->count += child_count;

            *child = node;

            trie->skip_addr &= MASK(trie->skip_len - lcp - 1);
            trie->skip_len  -= (lcp + 1);

        }
        else
        {
            mem_free(node);
        }
    }

    return rv;
}


/*
 * Function:
 *     _trie_v6_merge
 * Purpose:
 *     merge or fuse the child trie with parent trie
 */
int32
_trie_v6_merge(trie_node_t *parent_trie,
               trie_node_t *child_trie,
               uint32 *pivot,
               uint32 length,
               trie_node_t **new_parent)
{
    int32 rv, child_count;
    trie_node_t *child = NULL, clone;
    uint32 bpm[TAPS_MAX_KEY_SIZE_WORDS] = {0};
    uint32 child_pivot[BITS2WORDS(128)] = {0};
    uint32 child_length = 0;

    _trie_v6_search(child_trie, pivot, length, &child, child_pivot, &child_length, 0, 1);
    if (child == NULL) {
        return CTC_E_INVALID_PARAM;
    }

    _CLONE_TRIE_NODE_(&clone, child);

    if (child->type == PAYLOAD && child->bpm)
    {
        _TAPS_SET_KEY_BIT(bpm, 0, TAPS_IPV6_KEY_SIZE);
    }

    if (child != child_trie)
    {
        rv = _trie_v6_skip_node_free(child_trie, child_pivot, child_length);
        if (rv < 0)
        {
            return CTC_E_INVALID_PARAM;
        }
    }

    child_count = child->count;
    sal_memset(child, 0, sizeof(*child));

    rv = _trie_v6_insert(parent_trie, child_pivot, bpm, child_length, child,  new_parent, child_count);
    if (rv < 0)
    {
        return CTC_E_INVALID_PARAM;
    }

    if (child->child[0].child_node || child->child[1].child_node)
    {
        return CTC_E_INVALID_PARAM;
    }

    if (clone.type == INTERNAL)
    {
        child->type = INTERNAL;
    }

    child->child[0].child_node = clone.child[0].child_node;
    child->child[1].child_node = clone.child[1].child_node;

    return CTC_E_NONE;
}
#if 0
STATIC int32 _trie_v6_fuse_child(trie_node_t *trie, int32 bit)
{
    trie_node_t *child = NULL;
    int32 rv = CTC_E_NONE;

    if (trie->child[0].child_node && trie->child[1].child_node)
    {
        return CTC_E_INVALID_PARAM;
    }

    bit = (bit > 0)?1:0;
    child = trie->child[bit].child_node;

    if (child == NULL)
    {
        return CTC_E_INVALID_PARAM;
    }
    else
    {
        if (trie->skip_len + child->skip_len + 1 <= 31)
        {
            if (trie->skip_len == 0)
            {
                trie->skip_addr = 0;
            }

            if (child->skip_len < 31)
            {
                trie->skip_addr = SHL(trie->skip_addr, child->skip_len + 1, 31);
            }

            trie->skip_addr  |= SHL(bit, child->skip_len, 31);
            child->skip_addr |= trie->skip_addr;
            child->bpm       |= SHL(trie->bpm, child->skip_len + 1, 31);
            child->skip_len  += trie->skip_len + 1;

            if (trie->type == INTERNAL)
            {
                mem_free(trie);
            }
        }
    }

    return rv;
}


int32
_trie_v6_split2(trie_node_t *trie,
                uint32 *key,
                uint32 key_len,
                uint32 *pivot,
                uint32 *pivot_len,
                uint32 *split_count,
                trie_node_t **split_node,
                trie_node_t **child,
                trie_split2_states_e_t *state,
                const int32 max_split_count,
                const int32 exact_same)
{
    uint32 lcp = 0;
    int32 bit = 0, rv = CTC_E_NONE;

    /* start building the pivot */
    _key_append_v6(pivot, pivot_len, trie->skip_addr, trie->skip_len);

    lcp = _lcplen_v6(key, key_len, trie->skip_addr, trie->skip_len);

    if (lcp == trie->skip_len)
    {
        if (trie->count <= max_split_count && (!exact_same || (key_len - lcp) == 0))
        {
            *split_node = trie;
            *split_count = trie->count;
            if (trie->count < max_split_count)
            {
                *state = TRIE_SPLIT2_STATE_PRUNE_NODES;
            }

            return CTC_E_NONE;
        }

        if (key_len > lcp)
        {
            bit = (key[KEY_BIT2IDX(key_len - lcp)] & (1 << ((key_len - lcp - 1) &0x1F))) ? 1:0;

            if (trie->child[bit].child_node)
            {
                _key_append_v6(pivot, pivot_len, bit, 1);

                rv = _trie_v6_split2(trie->child[bit].child_node,
                                     key, key_len - lcp - 1,
                                     pivot, pivot_len, split_count,
                                     split_node, child, state,
                                     max_split_count, exact_same);
                if (NALPM_E_FAILURE(rv))
                {
                    return rv;
                }
            }
        }
    }

    /* free up internal nodes if applicable */
    switch(*state)
    {
    case TRIE_SPLIT2_STATE_NONE: /* fail to split */
        break;

    case TRIE_SPLIT2_STATE_PRUNE_NODES:
        if (trie->count == *split_count)
        {
            mem_free(trie);
        }
        else
        {
            trie->child[bit].child_node = NULL;
            bit = (bit == 0)?1:0;
            trie->count -= *split_count;

            /* optimize more */
            if ((trie->type == INTERNAL) && (trie->skip_len + trie->child[bit].child_node->skip_len + 1 <= 31))
            {
                *child = trie->child[bit].child_node;

                rv = _trie_v6_fuse_child(trie, bit);

                if (rv != CTC_E_NONE)
                {
                    *child = NULL;
                }
            }
            *state = TRIE_SPLIT2_STATE_DONE;
        }
        break;

    case TRIE_SPLIT2_STATE_DONE:
        if (*child != NULL)
        {
            trie->child[bit].child_node = *child;
            *child = NULL;
        }
        trie->count -= *split_count;
        break;

    default:
        break;
    }

    return rv;
}
#endif
#if 0
int32 _trie_v6_skip_node_alloc(trie_node_t **node,
                uint32 *key,
                uint32 *bpm,
                uint32 msb, /* NOTE: valid msb position 1 based, 0 means skip0/0 node */
                uint32 skip_len,
                trie_node_t *payload,
                uint32 count) /* payload count underneath - mostly 1 except some tricky cases */
{
    int32 lsb=0, msbpos=0, lsbpos=0, bit=0, index;
    trie_node_t *child = NULL, *skip_node = NULL;

    /* calculate lsb bit position, also 1 based */
    lsb = ((msb)? msb + 1 - skip_len : msb);

    if (msb)
    {
        for (index = BITS2SKIPOFF(lsb), lsbpos = lsb - 1; index <= BITS2SKIPOFF(msb); index++)
        {
            if (lsbpos == lsb - 1)
            {

                skip_node = payload;
            }
            else
            {
                skip_node = mem_malloc(MEM_IPUC_MODULE, sizeof(trie_node_t));
                if (NULL == skip_node)
                {
                    return CTC_E_NO_MEMORY;
                }
            }

            sal_memset(skip_node, 0, sizeof(trie_node_t));

            msbpos = index * 31 - 1;

            if (msbpos > msb - 1)
            {
                msbpos = msb - 1;
            }

            if (msbpos - lsbpos < 31)
            {
                skip_node->skip_len = msbpos - lsbpos + 1;
            }
            else
            {
                skip_node->skip_len = 31;
            }

            if (BITS2WORDS(msbpos + 1) != BITS2WORDS(lsbpos + 1))
            {
                /* pull snippets from the different words & fuse */
                skip_node->skip_addr = key[KEY_BIT2IDX(msbpos + 1)] & MASK((msbpos + 1) &0x1F);
                skip_node->skip_addr = SHL(skip_node->skip_addr, skip_node->skip_len - ((msbpos + 1) &0x1F), 32);
                skip_node->skip_addr |= SHR(key[KEY_BIT2IDX(lsbpos + 1)], (lsbpos &0x1F), 32);
            }
            else
            {
                skip_node->skip_addr = SHR(key[KEY_BIT2IDX(msbpos + 1)], (lsbpos &0x1F), 32);
            }

            /* set up the chain of child pointer, first node has no child since "child" was inited to NULL */
            if (child)
            {
                skip_node->child[bit].child_node = child;
            }

            /* calculate child pointer for next loop. NOTE: skip_addr has not been masked
            * so we still have the child bit in the skip_addr here.
            */
            bit = (skip_node->skip_addr & SHL(1, skip_node->skip_len - 1, 31)) ? 1:0;

            /* calculate node type */
            if (lsbpos == lsb - 1)
            {
                /* first node is payload */
                skip_node->type = PAYLOAD;
            }
            else
            {
                /* other nodes are internal nodes */
                skip_node->type = INTERNAL;
            }

            /* all internal nodes will have the same "count" as the payload node */
            skip_node->count = count;

            /* advance lsb to next word */
            lsbpos += skip_node->skip_len;


            /* for all child nodes 0/1 is implicitly obsorbed on parent */
            if (msbpos != msb - 1)
            {
                /* msbpos == (msb-1) is only true for the first node */
                skip_node->skip_len--;
            }
            skip_node->bpm &= MASK(skip_node->skip_len + 1);
            skip_node->skip_addr &= MASK(skip_node->skip_len);
            child = skip_node;
        }
    }
    else
    {
        skip_node = payload;
        sal_memset(skip_node, 0, sizeof(trie_node_t));
        skip_node->type = PAYLOAD;
        skip_node->count = count;

    }

    *node = skip_node;
    return CTC_E_NONE;
}
#endif

STATIC int32
trie_v6_traverse_propagate_prefix(trie_node_t    *trie,
                               trie_callback_f cb,
                               void  *user_data,
                               uint8 flag)
{
    int32 ret = 0;

    if (trie->type == PAYLOAD && (0 == flag))
    {
        ret = cb(trie, user_data);
        if (ret < 0)
        {
            return ret;
        }
    }

    if (trie->child[0].child_node && ret == 0)
    {
        ret = trie_v6_traverse_propagate_prefix(trie->child[0].child_node, cb, user_data, 0);
    }

    if (trie->child[1].child_node && ret == 0)
    {
        ret = trie_v6_traverse_propagate_prefix(trie->child[1].child_node, cb, user_data, 0);
    }

    return ret;
}


int32
sys_nalpm_v6_trie_propagate_prefix(trie_node_t   *trie,
                       uint32             *pfx,
                       uint32              len,
                       trie_callback_f     cb,
                       void               *user_data,
                       uint8              flag)
{
    int32 ret = 0;
    uint32 bit = 0, lcp = 0;

    lcp = _lcplen_v6(pfx, len, trie->skip_addr, trie->skip_len);

    if (trie->skip_len == lcp && lcp < len)
    {
        bit = (pfx[KEY_BIT2IDX (len - lcp)] & SHL (1, (len - lcp - 1) &0x1F, 32)) ? 1 : 0;

        if (trie->child[bit].child_node)
        {
            ret = sys_nalpm_v6_trie_propagate_prefix(trie->child[bit].child_node,  pfx, len - lcp - 1, cb, user_data, 0);
        }
    }
    else if (trie->skip_len >= lcp && lcp == len   )
    {
        ret = trie_v6_traverse_propagate_prefix(trie, cb, user_data, flag);
    }

    return ret;
}


int32
sys_trie_v6_find_lpm_prefix (trie_node_t * trie,
                             uint32 *pivot,
                             uint32 *length)
{
    if (!trie)
    {
        return 0;
    }

    if (trie->child[0].child_node == NULL && trie->child[1].child_node == NULL)
    {
        return 0;
    }

    _key_append_v6 (pivot, length, trie->skip_addr, trie->skip_len);


    if (trie->child[0].child_node && trie->child[1].child_node)
    {
        return 0;
    }

    if (trie->child[0].child_node)
    {
        _key_append_v6(pivot, length, 0, 1);
        sys_trie_v6_find_lpm_prefix(trie->child[0].child_node, pivot, length);

    }
    else if(trie->child[1].child_node)
    {
        _key_append_v6(pivot, length, 1, 1);
        sys_trie_v6_find_lpm_prefix(trie->child[1].child_node, pivot, length);
    }


    return 0;
}


STATIC int32
_trie_v6_find_lpm_sub_route(trie_node_t *trie,
                         uint32 *pivot,
                         uint32 *length,
                         trie_check_cb_f cb,
                         trie_chk_info_t *info)
{
    uint8 bit = 0;
    uint8 chd = 0;
    uint32 ip[4] = {0};
    trie_node_t * sub_trie = NULL;
    trie_dump_node_t *p_dump_node = info->dump_node;

    if (trie->child[0].child_node == NULL
        && trie->child[1].child_node == NULL)
    {
        SYS_NALPM_CONVERT_TO_MSB(pivot, *length, ip, 1);
        cb(info->lchip, info, ip);
    }
    else
    {
        for (chd = 0; chd < 2; chd++)
        {
            _key_append_v6(pivot, length, chd, 1);

            sub_trie = trie->child[chd].child_node;

            if (sub_trie == NULL)
            {
                SYS_NALPM_CONVERT_TO_MSB(pivot, *length, ip, 1);
                cb(info->lchip, info, ip);
            }
            else
            {
                trie_dump_node_t dump_node;
                sal_memcpy(&dump_node, p_dump_node, sizeof(trie_dump_node_t));
                dump_node.father = sub_trie;
                dump_node.father_lvl = p_dump_node->father_lvl + 1;
                info->dump_node = &dump_node;
                if (info->debug_en)
                {
                    info->cb(sub_trie, &dump_node);
                }

                if (sub_trie->type == PAYLOAD)
                {
                    if (sub_trie->skip_len > 0)
                    {
                        bit = (sub_trie->skip_addr >> (sub_trie->skip_len - 1));
                        _key_append_v6(pivot, length, bit == 0?1:0, 1);
                        SYS_NALPM_CONVERT_TO_MSB(pivot, *length, ip, 1);
                        cb(info->lchip, info, ip);
                        sys_nalpm_taps_key_shift(128, pivot, *length, 1);
                        *length = *length - 1;
                    }
                }
                else
                {
                    _key_append_v6(pivot, length, sub_trie->skip_addr, sub_trie->skip_len);
                    _trie_v6_find_lpm_sub_route(sub_trie, pivot, length, cb, info);
                    sys_nalpm_taps_key_shift(128, pivot, *length, sub_trie->skip_len);
                    *length = *length - sub_trie->skip_len;
                }
            }

            sys_nalpm_taps_key_shift(128, pivot, *length, 1);
            *length = *length - 1;

        }

    }

    return 0;
}


int32
_trie_v6_find_lpm_route (trie_node_t * trie,
                         uint32 *key,
                         uint32  key_len,
                         uint32 *pivot,
                         uint32 *length,
                         trie_check_cb_f cb,
                         trie_chk_info_t *info)
{
    uint32 lcp = 0;
    int32 bit = 0, rv = CTC_E_NONE;

    trie_dump_node_t *p_dump_node = info->dump_node;
    trie_dump_node_t dump_node;

    sal_memcpy(&dump_node, p_dump_node, sizeof(trie_dump_node_t));
    if (info->debug_en)
    {
        info->cb (trie, &dump_node);
    }

    lcp = _lcplen_v6 (key, key_len, trie->skip_addr, trie->skip_len);

    if ((key_len > trie->skip_len) && (lcp == trie->skip_len))
    {
        bit = (key[KEY_BIT2IDX(key_len - lcp)] & SHL(1, (key_len - lcp - 1) &0x1F,  32)) ? 1 : 0;

        _key_append_v6(pivot, length, trie->skip_addr, trie->skip_len);

        /* based on next bit branch left or right */
        if (trie->child[bit].child_node)
        {
            _key_append_v6(pivot, length, bit, 1);

            dump_node.father = trie;
            dump_node.father_lvl = p_dump_node->father_lvl + 1;
            info->dump_node = &dump_node;

            return _trie_v6_find_lpm_route (trie->child[bit].child_node, key, key_len - lcp - 1, pivot, length, cb, info);
        }
    }
    else if ((key_len == trie->skip_len) && (lcp == key_len))
    {
        if (trie->type == PAYLOAD)
        {
            /* exact match case */
            _key_append_v6(pivot, length, trie->skip_addr, trie->skip_len);

            _trie_v6_find_lpm_sub_route(trie, pivot, length, cb, info);
        }
    }
    return rv;
}
#endif

